import tellopy
import av
import cv2
import time
import numpy
import threading
import infer
import visualize
import math


class myTello(tellopy.Tello):
    def __init__(self):
        super(myTello, self).__init__()

        #建立连接
        self.connect()
        self.wait_for_connection(60.0)
        
        #图像存储
        self.Tello_frame=''
        self.framecurrent=[0]
        
        
        # 飞行状态
        self.AUTOLAND=False
        self.landingstate=-1

        try:
            self.container = av.open(self.get_video_stream())  # 解码视频流的容器
            print('container done')
        except:
            self.Tello_frame = 'decode error'       #解码失败的返回值
        self.ModelPath = 'inference_model\yolov3_darknet_voc'  # 模型文件默认路径
        self.detector = infer.Detector(self.ModelPath, use_gpu=False, run_mode='fluid')

        self.video_process_thread = threading.Thread(target=self.__Video_process)
        self.video_process_thread.setDaemon(True)
        self.vid_stream = self.container.streams.video[0]
        self.height = self.vid_stream.height
        self.width = self.vid_stream.width
        
        self.date_fmt = '%Y-%m-%d_%H%M%S'  # 日期格式

        self.video_process_thread.start()
        
        pass    
        
    def get_video_frame(self):
        '''返回当前的Tello图像'''
        return self.Tello_frame
    
    def __Video_process(self):
        '''单独线程，一直解码并处理来自无人机的视频流'''
        frame_skip = 300        #跳过前300帧
        n=0
        while True:
            for frame in self.container.decode(video=0):
                if 0 < frame_skip:
                    frame_skip = frame_skip - 1
                    continue
                start_time = time.time()
                self.Tello_frame = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
                # self.Tello_frame =numpy.array(frame.to_image())     #不需要色彩空间转换
                self.framecurrent[0]=self.Tello_frame
                # self.detection(n)
                # cv2.waitKey(20)
                if frame.time_base < 1.0 / 60:
                    time_base = 1.0 / 60
                else:
                    time_base = frame.time_base
                frame_skip = int((time.time() - start_time) / time_base)
                # print((time.time() - start_time), time_base)
                n+=1

    def detection(self):
        while True:
            try:
                img2=self.framecurrent[0]

                pred = self.self.detector.predict(self.Image, 0.5)
                img0 = visualize.visualize_box_mask(
                    self.img,
                    pred,
                    self.detector.config.labels,
                    mask_resolution=self.detector.config.mask_resolution)
                # if self.AUTOLAND:
                #     cv2.imwrite(str(n)+'.jpg',im0)
                cv2.imshow('cap', img0)
                cv2.waitKey(1)
                
                
                
                
                    
            except:
                continue

    def sigmond(self,x):
        return 1./(1.+numpy.exp(-x))

    def autoland(self):
      
        if self.landingstate==-1:
            n=0
            speed=5
            area=0
            sigmondalpha=5
            while True:
                position=self.detector.maxbbox
                try:
        
                    center=(int((position[0]+position[2])/2),int((position[1]+position[3])/2))
                    length=int(position[2]-position[0])
                    width=int(position[3]-position[1])
                    area = length*width

                    
                except:
                    center=[]
                    length=0
                    width=0
                    
                if center:
                    
                    # print('中心点坐标:(',center[0],',',center[1],')')
                    if center[0] > 485:
                        self.left(0)
                        # self.right(speed+abs(center[0]-480)*0.01)  #线性变速器
                        self.right(speed+sigmondalpha*self.sigmond(abs(center[0]-480)/60))
                        # print('向右')
                        # time.sleep(0.1)
                        # self.right(0)
                        
                    elif center[0] < 475:
                        self.right(0)
                        # self.left(speed+abs(center[0]-480)*0.01)
                        self.left(speed+sigmondalpha*self.sigmond(abs(center[0]-480)/60))
                        # print('向左')
                        # time.sleep(0.1)
                        # self.left(0)
                        
                    else:
                        self.left(0)
                        self.right(0)
                        
                    

                    if center[1] < 355:
                        self.down(0)
                        # self.up(speed+abs(center[1]-360)*0.01)
                        self.up(speed+sigmondalpha*self.sigmond(abs(center[1]-360)/45))
                        # print('向上')
                        # time.sleep(0.1)
                        # self.up(0)
                        
                    elif center[1] > 365:
                        self.up(0)
                        # self.down(speed+abs(center[1]-360)*0.01)
                        self.down(speed+sigmondalpha*self.sigmond(abs(center[1]-360)/45))
                        print('向下')
                        # time.sleep(0.1)
                        # self.down(0)
                        
                    else:
                        self.up(0)
                        self.down(0)
                        
                    
                    #if length>150:
                    if area > 10000:
                        speed=5
                    else:
                        speed=10
                    #if length>250:
                    if area >150000 or center[0]<200 or center[0]>760 or center[1] < 200 or center[1]>560:
                        self.forward(0)
                        self.backward(speed+5)
                        # self.backward(0)
                        
                    #elif length<50 and length>0:
                    elif area<140500 and area>0:
                        self.backward(0)
                        forwardspeed=speed+(400-math.sqrt(area))/40
                        self.forward(speed+(400-math.sqrt(area))/40)
                        print(forwardspeed,'\n',forwardspeed,'\n',forwardspeed,'\n',forwardspeed,'\n',forwardspeed,'\n',forwardspeed,'\n',forwardspeed,'\n')
                        # self.forward(0)
                        
                    else:
                        self.forward(0)
                        self.backward(0)
                        

                    if length<410:
                        n=0

                    else:
                        n+=1

                    if n>=10:
                        
                        self.land()
                        self.AUTOLAND=False
                        self.sock.close()     #关闭socket，避免下次打开时冲突
                        self.quit()
                        break



        pass
# if __name__ == '__main__':
#     tello=myTello()
    # tello.capshow()